import cv2
from picamera2 import Picamera2
import time
import numpy as np
import math
import serial

picam2 = Picamera2()
picam2.exposure_mode = 'off'
picam2.exposure_compensation = -2
picam2.awb_mode = 'off'
picam2.focus_mode = 'manual'
picam2.focus_distance = 0.5
picam2.start()

while True:

    frame = picam2.capture_array()
    
    frame_bgr = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)

    cv2.imshow("Camera Feed", frame_bgr)
    
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cv2.destroyAllWindows()
picam2.stop()
